// $Id: h264.c 111 2011-11-19 06:33:56Z cedric.shih@gmail.com $
/*
 * Copyright 2011 Cedric Shih (cedric dot shih at gmail dot com)
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 2.1 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 */

#include <stdlib.h>
#include <string.h>
#include <errno.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/tcp.h>

#include <zcbuffer.h>

#include "log.h"

#include "h264.h"

#define MIN(a,b) a == 0 ? b : (a > b ? b : a)
#define MAX(a,b) a > b ? a : b

int niortp_h264_nalu_offset(const unsigned char *data, size_t len,
		size_t *offset);

enum niortp_h264_nalu_type {
	NIORTP_H264_NON_IDR = 1,
	NIORTP_H264_IDR = 5,
	NIORTP_H264_SEI = 6,
	NIORTP_H264_SPS = 7,
	NIORTP_H264_PPS = 8,
};

enum niortp_h264_nalu_type niortp_h264_nalu_type(unsigned char header);

int niortp_h264_nalu_nri(unsigned char header);

int niortp_h264_put_queue(struct niortp_h264 *transport,
		const struct timeval *timestamp, struct zcbuffer *buffer,
		unsigned int priority);

struct zcbuffer *niortp_h264_take_queue(struct niortp_h264 *transport,
		struct timeval *timestamp);

size_t niortp_h264_queue_size(struct niortp_h264 *transport);

int niortp_h264_init(struct niortp_h264 *transport,
		const struct timeval *timestamp, struct zcbuffer *frame);

int niortp_h264_make_header(struct niortp_h264 *transport);

int niortp_h264_sendto(struct niortp_h264 *transport,
		const struct timeval *timestamp,
		struct zcbuffer *frame);

int niortp_h264_start(struct niortp_h264 *transport,
		const struct timeval *timestamp, struct zcbuffer *frame,
		enum niortp_h264_nalu_type type, unsigned int priority);

int niortp_h264_write_header(struct niortp_h264 *transport);

int niortp_h264_write_payload(struct niortp_h264 *transport);

ssize_t niortp_h264_clear_buffers(struct zcqueue *queue);

int niortp_h264_qos(struct niortp_h264 *transport,
		const struct timeval *timestamp, struct zcbuffer *frame,
		enum niortp_h264_nalu_type type, unsigned int priority);

void niortp_h264_stat_cb(int fd, short event, void *arg);

struct niortp_transport niortp_h264 = {
		niortp_h264_create,
		niortp_h264_destroy,
		niortp_h264_setup,
		niortp_h264_teardown,
		niortp_h264_send,
		niortp_h264_pending,
		niortp_h264_next,
		niortp_h264_stat,
};

void *niortp_h264_create(struct event_base *evbase)
{
	int rc;
	struct niortp_h264 *transport;

	if (!(transport = calloc(1, sizeof(*transport)))) {
		rc = errno ? errno : -1;
		niortsp_error("calloc: %s", strerror(rc));
		goto finally;
	}

	transport->evbase = evbase;

	rc = 0;

finally:
	if (rc != 0) {
		niortp_h264_destroy(transport);
		transport = NULL;
		errno = rc;
	}
	return transport;
}

void niortp_h264_destroy(void *obj)
{
	struct niortp_h264 *transport = obj;

	if (!transport) {
		return;
	}

	if (transport->setup) {
		niortp_h264_teardown(transport);
	}

	free(transport);
}

int niortp_h264_setup(void *obj, int fd, struct niortp_transport_param *param)
{
	int rc, i;
	struct niortp_h264 *transport = obj;

	if (!transport || !param) {
		rc = EINVAL;
		niortsp_error("null pointer(s)");
		goto finally;
	}

	if (transport->setup) {
		rc = EPERM;
		niortsp_error("already set up");
		goto finally;
	}

	niortsp_debug("setting up: %s:%d", inet_ntoa(param->remote.sin_addr),
			ntohs(param->remote.sin_port));

	switch(param->type) {
	case NIORTP_TRANSPORT_UDP_UNICAST:
		break;
	case NIORTP_TRANSPORT_INTERLEAVED:
		for (i = 0; i < H264_NRI_MAX; i++) {
			if (!(transport->queues[i] = zcqueue_new())) {
				rc = errno ? errno : -1;
				niortsp_error("zcqueue_new: %s",
						strerror(rc));
				goto finally;
			}
		}

		if (!(transport->dropping = zcqueue_new())) {
			rc = errno ? errno : -1;
			niortsp_error("zcqueue_new: %s", strerror(rc));
			goto finally;
		}
		break;
	default:
		rc = ENOSYS;
		niortsp_error("transport not supported: %d", param->type);
		break;
	}

	evtimer_set(&transport->ev, niortp_h264_stat_cb, transport);

	if (event_base_set(transport->evbase, &transport->ev)) {
		rc = errno ? errno : -1;
		niortsp_error("failed to set event base: %s", strerror(rc));
		goto finally;
	}

	transport->tv.tv_sec = 1;
	transport->tv.tv_usec = 0;

	if (event_add(&transport->ev, &transport->tv)) {
		rc = errno ? errno : -1;
		niortsp_error("failed to add timer: %s", strerror(rc));
		goto finally;
	}

	transport->fd = fd;
	memcpy(&transport->param, param, sizeof(transport->param));
	transport->setup = 1;

	rc = 0;

finally:
	return rc;
}

int niortp_h264_teardown(void *obj)
{
	int rc, i;
	ssize_t n;
	struct niortp_h264 *transport = obj;

	if (!transport) {
		rc = EINVAL;
		niortsp_error("null pointer(s)");
		goto finally;
	}

	if (!transport->setup) {
		rc = EPERM;
		niortsp_error("not yet set up");
		goto finally;
	}

	if (event_initialized(&transport->ev) &&
			evtimer_pending(&transport->ev, NULL)) {
		if ((rc = evtimer_del(&transport->ev))) {
			niortsp_error("failed to delete timer: %s",
					strerror(rc));
			goto finally;
		}
	}

	if (transport->buffer) {
		zcbuffer_free(transport->buffer);
		transport->buffer = NULL;
	}

	if (transport->dropping) {
		if ((n = niortp_h264_clear_buffers(
				transport->dropping)) < 0) {
			rc = errno ? errno : -1;
			niortsp_error("failed to clear buffers: %s",
					strerror(rc));
			goto finally;
		}

		zcqueue_free(transport->dropping);
		transport->dropping = NULL;
	}

	for (i = 0; i < H264_NRI_MAX; i++) {
		if (transport->queues[i]) {
			if ((n = niortp_h264_clear_buffers(
					transport->queues[i])) < 0) {
				rc = errno ? errno : -1;
				niortsp_error("failed to clear: %s",
						strerror(rc));
				goto finally;
			}

			zcqueue_free(transport->queues[i]);
			transport->queues[i] = NULL;
		}
	}

	transport->setup = 0;
	rc = 0;

finally:
	return rc;
}

int niortp_h264_stat(void *obj, struct niortp_transport_stat *stat)
{
	int rc;
	struct niortp_h264 *transport = obj;

	if (!transport || !stat) {
		rc = EINVAL;
		niortsp_error("null pointer(s)");
		goto finally;
	}

	if (!transport->setup) {
		rc = EPERM;
		niortsp_error("not yet set up");
		goto finally;
	}

	memcpy(stat, &transport->old, sizeof(*stat));
	rc = 0;

finally:
	return rc;
}

int niortp_h264_send(void *obj, const struct timeval *timestamp,
		struct zcbuffer *frame)
{
	int rc, header, nri;
	size_t offset;
	enum niortp_h264_nalu_type type;
	struct niortp_h264 *transport = obj;

	if (!transport || !timestamp || !frame) {
		rc = EINVAL;
		niortsp_error("null pointer(s)");
		goto finally;
	}

	if (!transport->setup) {
		rc = EPERM;
		niortsp_error("not yet set up");
		goto finally;
	}

	transport->new.incoming.units++;
	transport->new.incoming.bytes += zcbuffer_remaining(frame);

	if ((rc = niortp_h264_nalu_offset(zcbuffer_array(frame),
			zcbuffer_remaining(frame), &offset))) {
		niortsp_error("failed to resolve offset: %s", strerror(rc));
		goto finally;
	}

	if ((rc = zcbuffer_skip(frame, offset))) {
		niortsp_error("failed to skip offset '%d': %s", offset,
				strerror(rc));
		goto finally;
	}

	niortsp_trace("sending frame %p to: %s:%d", frame,
			inet_ntoa(transport->param.remote.sin_addr),
			ntohs(transport->param.remote.sin_port));

	if ((rc = zcbuffer_peek(frame, zcbuffer_position(frame), &header))) {
		niortsp_error("failed to peek NAL header: %s",
				strerror(rc));
		goto finally;
	}

	type = niortp_h264_nalu_type(header);
	nri = niortp_h264_nalu_nri(header);

	niortsp_debug("type=%d, nri=%d, size=%d", type, nri,
			zcbuffer_remaining(frame));

	if ((rc = niortp_h264_qos(transport, timestamp, frame,
			type, nri))) {
		niortsp_error("failed to apply QoS rules: %s", strerror(rc));
		goto finally;
	}

	if (!transport->refreshed) {
		switch (type) {
		case NIORTP_H264_NON_IDR:
			niortsp_trace("skipping until refreshed...");
			transport->new.dropped.units++;
			transport->new.dropped.bytes += zcbuffer_remaining(frame);
			rc = 0;
			goto finally;
		case NIORTP_H264_IDR:
			transport->refreshed = 1;
			break;
		default:
			break;
		}
	}

	switch(transport->param.type) {
	case NIORTP_TRANSPORT_UDP_UNICAST:
		if ((rc = niortp_h264_sendto(transport, timestamp,
				frame))) {
			niortsp_error("failed to unicast: %s",
					strerror(rc));
			goto finally;
		}
		break;
	case NIORTP_TRANSPORT_INTERLEAVED:
		if ((rc = niortp_h264_start(transport, timestamp,
				frame, type, nri))) {
			niortsp_error("failed to send frame: %s",
					strerror(rc));
			goto finally;
		}
		break;
	default:
		rc = ENOSYS;
		niortsp_error("transport not supported: %d",
				transport->param.type);
		goto finally;
	}

	rc = 0;

finally:
	return rc;
}

int niortp_h264_pending(void *obj)
{
	int pending;
	struct niortp_h264 *transport = obj;

	if (!transport) {
		niortsp_error("null pointer");
		return 0;
	}

	if ((pending = (transport->param.type ==
			NIORTP_TRANSPORT_INTERLEAVED) &&
			(transport->buffer ||
			niortp_h264_queue_size(transport) > 0))) {
		niortsp_trace("pending");
	} else {
		niortsp_trace("not pending");
	}

	return pending;
}

int niortp_h264_next(void *obj)
{
	int rc;
	struct niortp_h264 *transport = obj;
	struct zcbuffer *buffer = NULL;
	struct timeval timestamp;

	if (!transport) {
		rc = EINVAL;
		niortsp_error("null pointer");
		goto finally;
	}

	if (transport->param.type != NIORTP_TRANSPORT_INTERLEAVED) {
		rc = EPERM;
		niortsp_error("not interleaved");
		goto finally;
	}

	if (!transport->buffer) {
		niortsp_trace("processing next frame");

		if (!(buffer = niortp_h264_take_queue(transport,
				&timestamp))) {
			rc = errno ? errno : -1;
			niortsp_error("failed to take: %s", strerror(rc));
			goto finally;
		}

		if ((rc = niortp_h264_init(transport, &timestamp,
				buffer))) {
			niortsp_error("failed to init: %s", strerror(rc));
			goto finally;
		}

		if ((rc = niortp_h264_make_header(transport))) {
			niortsp_error("failed to make header: %s",
					strerror(rc));
			goto finally;
		}
	}

	niortsp_trace("continuing H.264 frame: %p", transport->buffer);

	if (transport->packet.header.written < transport->packet.header.len &&
			(rc = niortp_h264_write_header(transport))) {
		if (rc != EAGAIN && rc != EINPROGRESS) {
			niortsp_error("failed to write header: %s",
					strerror(rc));
		}
		goto finally;
	}

	if ((rc = niortp_h264_write_payload(transport))) {
		if (rc != EAGAIN && rc != EINPROGRESS) {
			niortsp_error("failed to write payload: %s",
					strerror(rc));
		}
		goto finally;
	}

	rc = 0;

finally:
	return rc;
}

int niortp_h264_nalu_offset(const unsigned char *data, size_t len,
		size_t *offset)
{
	int rc;
	size_t i, max;
	unsigned char magic[] = { 0x00, 0x00, 0x01 };

	if (len <= 0) {
		rc = EINVAL;
		niortsp_error("empty data");
		goto finally;
	}

	if (data[0] != 0) {
		*offset = 0;
	} else { // search for Annex B prefix
		if (len <= sizeof(magic)) {
			rc = EINVAL;
			niortsp_error("buffer underflows");
			goto finally;
		}

		max = len - sizeof(magic);

		for (i = 0; i < max; i++) {
			if (memcmp(data + i, magic, sizeof(magic))) {
				continue;
			}

			*offset = i + sizeof(magic);
			break;
		}

		if (i == max) {
			rc = EINVAL;
			niortsp_error("magic not found");
			goto finally;
		}
	}

	rc = 0;

finally:
	return rc;
}

inline enum niortp_h264_nalu_type niortp_h264_nalu_type(unsigned char header)
{
	return header & 0x1f;
}

inline int niortp_h264_nalu_nri(unsigned char header)
{
	return (header >> 5) & 0x03;
}

int niortp_h264_init(struct niortp_h264 *transport,
		const struct timeval *timestamp, struct zcbuffer *buffer)
{
	int rc, header;
	struct timeval now, diff;
	unsigned int duration;

	if ((rc = zcbuffer_get(buffer, &header))) {
		niortsp_error("failed to get NAL header: %s", strerror(rc));
		goto finally;
	}

	transport->nal_header = header & 0xff;

	memset(&transport->header, '\0', sizeof(transport->header));

	transport->header.interleaved.magic = '$';
	transport->header.interleaved.stream = ntohs(
			transport->param.address.rtp.sin_port);

	transport->header.rtp.a = 0x80;
	transport->header.rtp.b = transport->param.payload_type;
	transport->header.rtp.ts = htonl(
			niortp_timeval_to_timestamp(timestamp));
	transport->header.rtp.ssrc = (u_int32_t)transport;

	if (memcmp(&transport->timestamp, timestamp,
			sizeof(transport->timestamp))) {
		if ((rc = gettimeofday(&now, NULL)) == -1) {
			rc = errno ? errno : -1;
			niortsp_error("failed to get time: %s", strerror(rc));
			goto finally;
		}

		if (transport->start.tv_sec != 0 &&
				transport->start.tv_usec != 0) {
			if ((rc = niortp_timeval_diff(&transport->start,
					&now, &diff))) {
				niortsp_error("failed to diff time: %s",
						strerror(rc));

				memset(&transport->start, '\0',
						sizeof(transport->start));
			} else {
				duration = diff.tv_sec * 1000000 + diff.tv_usec;
				transport->new.duration.min = MIN(
						transport->new.duration.min,
						duration);
				transport->new.duration.max = MAX(
						transport->new.duration.max,
						duration);
			}
		}

		memcpy(&transport->start, &now, sizeof(transport->start));
	}

	memcpy(&transport->timestamp, timestamp, sizeof(transport->timestamp));
	transport->buffer = buffer;
	transport->offset = 0;
	rc = 0;

finally:
	return rc;
}

int niortp_h264_make_header(struct niortp_h264 *transport)
{
	int rc, tcp, marker;
	size_t remaining, header, payload;
	ssize_t available;

	niortsp_trace("making header: offset=%d", transport->offset);

	tcp = transport->param.type == NIORTP_TRANSPORT_INTERLEAVED;

	remaining = zcbuffer_remaining(transport->buffer);
	header = (tcp ? sizeof(transport->header.interleaved) : 0) +
			 sizeof(transport->header.rtp);
	available = transport->param.mtu - IP_HEADER_MAX - (tcp ?
			TCP_HEADER_MAX : UDP_HEADER_MAX) - header;
	available = (available >> 2) << 2;

	if (available < 0) {
		rc = EOVERFLOW;
		niortsp_error("insufficient MTU size: %d", transport->param.mtu);
		goto finally;
	}

	if (available < remaining || transport->offset != 0) {
		header += sizeof(transport->header.fu);
		available -= sizeof(transport->header.fu);
	} else {
		header += sizeof(transport->header.nal);
		available -= sizeof(transport->header.nal);
	}

	marker = available >= remaining;
	payload = remaining > available ? available : remaining;

	if (tcp) {
		transport->header.interleaved.len = htons(header -
				sizeof(transport->header.interleaved) +
				payload);
	}

        transport->header.rtp.seq = htons(transport->sequence++);
        transport->new.rtp.units++;

	if (marker) {
		niortsp_trace("turning on marker bit");
		transport->header.rtp.b |= 0x80; // Marker=1
	}

	if (available < remaining || transport->offset != 0) {
		transport->header.fu.indicator = transport->nal_header & 0x60; // F and NRI
		transport->header.fu.indicator |= 28; // FU-A
		transport->header.fu.header = transport->nal_header & 0x1f; // NAL type

		if (transport->offset == 0) {
			transport->header.fu.header |= 0x80; // S(tart)
		}

		if (marker) {
			transport->header.fu.header |= 0x40; // E(nd)
		}
	} else {
		transport->header.nal = transport->nal_header;
	}

	transport->packet.header.len = header;
	transport->packet.header.written = 0;
	transport->packet.len = payload;
	transport->packet.written = 0;
	rc = 0;

finally:
	return rc;
}

int niortp_h264_sendto(struct niortp_h264 *transport,
		const struct timeval *timestamp, struct zcbuffer *frame)
{
	int rc, fd;
	struct sockaddr_in *addr;
	unsigned char *data;
	size_t len;

	niortsp_trace("sending frame %p: %s:%d", frame,
			inet_ntoa(transport->param.remote.sin_addr),
			ntohs(transport->param.remote.sin_port));

	if ((rc = niortp_h264_init(transport, timestamp, frame))) {
		niortsp_error("failed to init: %s", strerror(rc));
		goto finally;
	}

	fd = transport->fd;
	addr = &transport->param.address.rtp;

	while ((zcbuffer_remaining(frame)) > 0) {
		if ((rc = niortp_h264_make_header(transport))) {
			niortsp_error("failed to make header: %s",
					strerror(rc));
			goto finally;
		}

		data = (unsigned char *)&transport->header.rtp;
		len = transport->packet.header.len;

		niortsp_trace("sending header: %d@%p", len, data);

		if (sendto(fd, data, len, MSG_DONTWAIT|MSG_MORE,
				(struct sockaddr *)addr,
				sizeof(*addr)) != len) {
			rc = errno ? errno : -1;
			niortsp_error("failed to unicast: %s", strerror(rc));
			goto finally;
		}

		transport->new.rtp.bytes += len;

		data = zcbuffer_array(frame) + zcbuffer_position(frame);
		len = transport->packet.len;

		niortsp_trace("sending data: %d@%p", len, data);

		if (sendto(fd, data, len, MSG_DONTWAIT,
				(struct sockaddr *)addr,
				sizeof(*addr)) != len) {
			rc = errno ? errno : -1;
			niortsp_error("failed to write data: %s", strerror(rc));
			goto finally;
		}

		transport->new.rtp.bytes += len;

		if ((rc = zcbuffer_skip(frame, len))) {
			niortsp_error("failed to skip: %s", strerror(rc));
			goto finally;
		}

		transport->offset += len;
	}

	if (memcmp(&transport->previous, &transport->timestamp,
			sizeof(transport->previous))) {
		transport->new.instances++;
		memcpy(&transport->previous, &transport->timestamp,
				sizeof(transport->previous));
	}

	transport->buffer = NULL;
	transport->new.outgoing.units++;
	transport->new.outgoing.bytes += transport->offset + 1; // 1=NAL header

	rc = 0;

finally:
	return rc;
}

int niortp_h264_qos(struct niortp_h264 *transport,
		const struct timeval *timestamp, struct zcbuffer *frame,
		enum niortp_h264_nalu_type type, unsigned int priority)
{
	int rc, i;
	size_t size, dropped[H264_NRI_MAX], queued[H264_NRI_MAX];
	ssize_t n;

	if (transport->param.type == NIORTP_TRANSPORT_UDP_UNICAST) {
		niortsp_trace("no need for apply Qos");
		rc = 0;
		goto finally;
	}

	size = 0;
	memset(&queued, '\0', sizeof(queued));

	for (i = 0; i < H264_NRI_MAX; i++) {
		queued[i] = zcqueue_size(transport->queues[i]);

		if (i < priority) {
			size += queued[i];
		}
	}

	niortsp_trace("applying QoS rules");

	memset(&dropped, '\0', sizeof(dropped));

	for (i = 0; i < priority; i++) {
		niortsp_trace("clearing buffer(s) in queue of priority: %d", i);

		if ((n = niortp_h264_clear_buffers(
				transport->queues[i])) < 0) {
			rc = errno ? errno : -1;
			niortsp_error("failed to clear buffers: %s",
					strerror(rc));
			goto finally;
		}

		transport->new.dropped.units += queued[i];
		transport->new.dropped.bytes += n;
		dropped[i] = queued[i];
		queued[i] = 0;
	}

	if (transport->buffer) {
		for (i = priority; i < H264_NRI_MAX; i++) {
			niortsp_trace("clearing frame(s) newer than %ld.%06ld "
					"in queue of the same priority",
					transport->timestamp.tv_sec,
					transport->timestamp.tv_usec);

			if ((rc = zcqueue_take_newer(
					transport->queues[i],
					transport->dropping,
					&transport->timestamp))) {
				niortsp_error("failed to take newer: %s",
						strerror(rc));
				goto finally;
			}

			dropped[i] = zcqueue_size(transport->dropping);

			if ((n = niortp_h264_clear_buffers(
					transport->dropping)) < 0) {
				rc = errno ? errno : -1;
				niortsp_error("failed to clear: %s",
						strerror(rc));
				goto finally;
			}

			queued[i] -= dropped[i];
			size += dropped[i];
			transport->new.dropped.units += dropped[i];
			transport->new.dropped.bytes += n;
		}
	}

	if (size > 0) {
		niortsp_warn("qos applied %s:%d => dropped={ %d, %d, %d, %d }, "
				"queued={ %d, %d, %d, %d }",
				inet_ntoa(transport->param.remote.sin_addr),
				ntohs(transport->param.remote.sin_port),
				dropped[0], dropped[1], dropped[2], dropped[3],
				queued[0], queued[1], queued[2], queued[3]);

		transport->refreshed = 0;
	}

	rc = 0;

finally:
	return rc;
}

int niortp_h264_start(struct niortp_h264 *transport,
		const struct timeval *timestamp, struct zcbuffer *frame,
		enum niortp_h264_nalu_type type, unsigned int priority)
{
	int rc;
	struct zcbuffer *clone = NULL;

	niortsp_trace("streaming frame %p: %s:%d", frame,
			inet_ntoa(transport->param.remote.sin_addr),
			ntohs(transport->param.remote.sin_port));

	if (!(clone = zcbuffer_clone(frame))) {
		rc = errno ? errno : -1;
		niortsp_error("failed to clone: %s",
				strerror(rc));
		goto finally;
	}

	if ((rc = niortp_h264_put_queue(transport, timestamp, clone,
			priority))) {
		niortsp_error("failed to put: %s", strerror(rc));
		goto finally;
	}

	rc = 0;

finally:
	if (rc != 0) {
		if (clone) {
			zcbuffer_free(clone);
		}
	}
	return rc;
}

int niortp_h264_write_header(struct niortp_h264 *transport)
{
	int rc;
	ssize_t n;
	void *data;
	size_t len;

	niortsp_trace("writing header: %d/%d bytes",
			transport->packet.header.written,
			transport->packet.header.len);

	data = &transport->header + transport->packet.header.written;
	len = transport->packet.header.len - transport->packet.header.written;

	if ((n = write(transport->fd, data, len)) <= 0) {
		rc = errno ? errno : -1;

		if (rc != EAGAIN && rc != EINPROGRESS) {
			niortsp_error("failed to write: %s", strerror(rc));
		}

		goto finally;
	}

	niortsp_trace("header written: %d bytes", n);

	transport->packet.header.written += n;
	transport->new.rtp.bytes += n;
	rc = 0;

finally:
	return rc;
}

int niortp_h264_write_payload(struct niortp_h264 *transport)
{
	int rc;
	ssize_t n;
	unsigned char *data;
	size_t len;

	if (zcbuffer_remaining(transport->buffer) <= 0) {
		rc = ENODATA;
		niortsp_error("buffer already written");
		goto finally;
	}

	niortsp_trace("writing payload: %d/%d bytes",
			transport->packet.written, transport->packet.len);

	data = zcbuffer_array(transport->buffer) +
			zcbuffer_position(transport->buffer) +
			transport->packet.written;
	len = transport->packet.len - transport->packet.written;

	if ((n = write(transport->fd, data, len)) <= 0) {
		rc = errno ? errno : -1;

		if (rc != EAGAIN && rc != EINPROGRESS) {
			niortsp_error("failed to write: %s", strerror(rc));
		}

		goto finally;
	}

	niortsp_trace("payload written: %d bytes", n);

	transport->packet.written += n;
	transport->new.rtp.bytes += n;

	if (transport->packet.written >= transport->packet.len) {
		niortsp_trace("packet transport completed; incrementing "
				"position: %d => %d",
				zcbuffer_position(transport->buffer),
				zcbuffer_position(transport->buffer) +
				transport->packet.len);

		if ((rc = zcbuffer_skip(transport->buffer,
				transport->packet.len))) {
			niortsp_error("failed to skip: %s", strerror(rc));
			goto finally;
		}

		transport->offset += transport->packet.len;

		if (zcbuffer_remaining(transport->buffer) > 0) {
			niortsp_trace("preparing next packet");

			if ((rc = niortp_h264_make_header(transport))) {
				niortsp_error("failed to make header: %s",
						strerror(rc));
				goto finally;
			}
		} else {
			niortsp_trace("frame transport completed: %p",
					transport->buffer);

			zcbuffer_free(transport->buffer);
			transport->buffer = NULL;
			transport->new.outgoing.units++;
			transport->new.outgoing.bytes += transport->offset + 1; // 1=NAL header

			if (memcmp(&transport->previous,
					&transport->timestamp,
					sizeof(transport->previous))) {
				transport->new.instances++;
				memcpy(&transport->previous,
						&transport->timestamp,
						sizeof(transport->previous));
			}
		}
	}

	rc = 0;

finally:
	return rc;
}

ssize_t niortp_h264_clear_buffers(struct zcqueue *queue)
{
	int rc;
	struct zcbuffer *buffer;
	ssize_t size = 0;

	while (zcqueue_size(queue) > 0) {
		if (!(buffer = zcqueue_take(queue, NULL))) {
			rc = errno ? errno : -1;
			niortsp_error("failed to take: %s", strerror(rc));
			goto finally;
		}
		size += zcbuffer_remaining(buffer) + 1; // 1 = NAL header
		zcbuffer_free(buffer);
	}

	rc = 0;

finally:
	if (rc != 0) {
		size = -1;
		errno = rc;
	}
	return size;
}

int niortp_h264_put_queue(struct niortp_h264 *transport,
		const struct timeval *timestamp, struct zcbuffer *buffer,
		unsigned int priority)
{
	int rc;

	if (priority < 0 || priority >= H264_NRI_MAX) {
		rc = EINVAL;
		niortsp_error("index out of bound");
		goto finally;
	}

	if ((rc = zcqueue_put(transport->queues[priority],
			timestamp, buffer))) {
		niortsp_error("failed to put: %s", strerror(rc));
		goto finally;
	}

	rc = 0;

finally:
	return rc;
}

struct zcbuffer *niortp_h264_take_queue(struct niortp_h264 *transport,
		struct timeval *timestamp)
{
	int rc = ENOENT, i;
	struct zcbuffer *buffer = NULL;

	for (i = H264_NRI_MAX - 1; i >= 0; i--) {
		if (zcqueue_size(transport->queues[i]) <= 0) {
			continue;
		}

		if (!(buffer = zcqueue_take(transport->queues[i],
				timestamp))) {
			niortsp_error("failed to take: %s", strerror(rc));
			goto finally;
		}

		break;
	}

	rc = 0;

finally:
	if (rc != 0) {
		buffer = NULL;
		errno = rc;
	}
	return buffer;
}

size_t niortp_h264_queue_size(struct niortp_h264 *transport)
{
	int i;
	size_t size = 0;

	for (i = H264_NRI_MAX - 1; i >= 0; i--) {
		if (!transport->queues[i]) {
			continue;
		}
		size += zcqueue_size(transport->queues[i]);
	}

	return size;
}

void niortp_h264_stat_cb(int fd, short event, void *arg)
{
	struct niortp_h264 *transport = arg;

	memcpy(&transport->old, &transport->new, sizeof(transport->old));
	memset(&transport->new, '\0', sizeof(transport->new));
	event_add(&transport->ev, &transport->tv);
}
